#! /usr/bin/env python
# Copyright (c) 2013-2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from dynamic_reconfigure.parameter_generator_catkin import (
    ParameterGenerator,
    double_t,
)

gen = ParameterGenerator()

gen.add(
    'goal_time', double_t, 0,
    "Amount of time (s) controller is permitted to be late achieving goal",
    0.1, 0.0, 120.0,
)
gen.add(
    'stopped_velocity_tolerance', double_t, 0,
    "Maximum velocity (m/s) at end of trajectory to be considered stopped",
    0.25, -1.0, 1.0,
)

joints = ('right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4','right_j5', 'right_j6',)

params = ('_goal', '_trajectory',)
msg = (
    " - maximum final error",
    " - maximum error during trajectory execution",
    )
min = (-1.0, -1.0,)
default = (-1.0, 0.2,)
max = (1.0, 1.0,)

for idx, param in enumerate(params):
    for joint in joints:
        gen.add(
            joint + param, double_t, 0, joint + msg[idx],
            default[idx], min[idx], max[idx]
        )

exit(gen.generate('intera_interface', '', 'SawyerPositionJointTrajectoryActionServer'))
